#include "stdafx.h"
#include "Point3d.h"
#include "Vector3d.h"
#include "Matrix3d.h"
#include "GeomUtils.h"

using namespace stl;

CPoint3D::CPoint3D()
{
	x = 0.0;
	y = 0.0;
	z = 0.0;
}

CPoint3D::CPoint3D(double _x, double _y, double _z)
{
	x = _x;
	y = _y;
	z = _z;
}

CPoint3D::CPoint3D(const double *pPt)
{
	x = 0.0;
	y = 0.0;
	z = 0.0;
	if (pPt != nullptr)
	{
		x = pPt[0];
		y = pPt[1];
		z = pPt[2];
	}
}

CPoint3D::CPoint3D(const POINT3D & p)
{
	x = p.x;
	y = p.y;
	z = p.z;
}

CPoint3D::CPoint3D(const CPoint3D & pt)
{
	x = pt.x;
	y = pt.y;
	z = pt.z;
}

CPoint3D::~CPoint3D()
{
}

CPoint3D CPoint3D::operator*(const MATRIX3d& matrix) const
{
	double rx, ry, rz, sc;
	rx = x * matrix.A[0][0] + y * matrix.A[1][0] + z * matrix.A[2][0] + matrix.A[3][0];
	ry = x * matrix.A[0][1] + y * matrix.A[1][1] + z * matrix.A[2][1] + matrix.A[3][1];
	rz = x * matrix.A[0][2] + y * matrix.A[1][2] + z * matrix.A[2][2] + matrix.A[3][2];
	sc = x * matrix.A[0][3] + y * matrix.A[1][3] + z * matrix.A[2][3] + matrix.A[3][3];
	rx /= sc;
	ry /= sc;
	rz /= sc;
	return CPoint3D(rx, ry, rz);
}

void CPoint3D::operator*=(const MATRIX3d & matrix)
{
	*this = (*this) * matrix;
}

CPoint3D CPoint3D::operator+(VECTOR3D v) const
{
	return CPoint3D(x + v.x, y + v.y, z + v.z);
}

void CPoint3D::operator+=(VECTOR3D v)
{
	x += v.x;
	y += v.y;
	z += v.z;
}

CPoint3D CPoint3D::operator-(VECTOR3D v) const
{
	CPoint3D pt(x, y, z);
	pt.x -= v.x;
	pt.y -= v.y;
	pt.z -= v.z;
	return pt;
}

void CPoint3D::operator-=(VECTOR3D v)
{
	x -= v.x;
	y -= v.y;
	z -= v.z;
}

CVector3D CPoint3D::operator-(POINT3D sp) const
{
	return CVector3D(x - sp.x, y - sp.y, z - sp.z);
}

bool CPoint3D::operator==(POINT3D pos) const
{
	if (GeEqual(x, pos.x)
		&& GeEqual(y, pos.y)
		&& GeEqual(z, pos.z))
	{
		return true;
	}
	return false;
}

bool CPoint3D::operator!=(POINT3D pos) const
{
	if (!GeEqual(x, pos.x)
		|| !GeEqual(y, pos.y)
		|| !GeEqual(z, pos.z))
	{
		return true;
	}
	return false;
}

double stl::CPoint3D::DistTo(POINT3D pt, bool bXYPlane /*= true*/) const
{
	double dZ = bXYPlane ? 0.0 : pt.z - z;
	CVector3D vec(pt.x - x, pt.y - y, dZ);
	return vec.GetLength();
}

void stl::CPoint3D::set(double _x, double _y, double _z)
{
	x = _x;
	y = _y;
	z = _z;
}